#include "Matrix4x4.h"

namespace lib3dw
{
	void Matrix4x4::translate(const Vector3 &v)
	{
		rows[0].w += dot(rows[0].xyz(), v);
		rows[1].w += dot(rows[1].xyz(), v);
		rows[2].w += dot(rows[2].xyz(), v);
		rows[3].w += dot(rows[3].xyz(), v);
	}
	
	Matrix4x4 Matrix4x4::translate(const float x, const float y, const float z)
	{
		return Matrix4x4(
			1.0, 0.0, 0.0, x  , 
			0.0, 1.0, 0.0, y  , 
			0.0, 0.0, 1.0, z  , 
			0.0, 0.0, 0.0, 1.0);
	}
	
	Matrix4x4 Matrix4x4::scale(const float x, const float y, const float z)
	{
		return Matrix4x4(
			x  , 0.0, 0.0, 0.0, 
			0.0, y  , 0.0, 0.0, 
			0.0, 0.0, z  , 0.0, 
			0.0, 0.0, 0.0, 1.0);
	}
	
	Matrix4x4 Matrix4x4::rotateX(float a)
	{
		return Matrix4x4(
			1.0,	0.0,	0.0,	0.0, 
			0.0,	cos(a),-sin(a), 0.0, 
			0.0,	sin(a),	cos(a),	0.0, 
			0.0,	0.0,	0.0,	1.0);
	}

	Matrix4x4 Matrix4x4::rotateY(float a)
	{
		return Matrix4x4(
			cos(a),	0.0,	sin(a),	0.0, 
			0.0,	1.0,	0.0,	0.0, 
		   -sin(a),	0.0,	cos(a),	0.0, 
			0.0,	0.0,	0.0,	1.0);
	}

	Matrix4x4 Matrix4x4::rotateZ(float a)
	{
		return Matrix4x4(
			cos(a),-sin(a),	0.0,	0.0, 
			sin(a),	cos(a),	0.0,	0.0, 
			0.0,	0.0,	1.0,	0.0, 
			0.0,	0.0,	0.0,	1.0);
	}

	Matrix4x4 operator + (const Matrix4x4 &m, const Matrix4x4 &n)
	{
		return Matrix4x4(m.rows[0] + n.rows[0], m.rows[1] + n.rows[1], m.rows[2] + n.rows[2], m.rows[3] + n.rows[3]);
	}

	Matrix4x4 operator - (const Matrix4x4 &m, const Matrix4x4 &n)
	{
		return Matrix4x4(m.rows[0] - n.rows[0], m.rows[1] - n.rows[1], m.rows[2] - n.rows[2], m.rows[3] - n.rows[3]);
	}

	Matrix4x4 operator - (const Matrix4x4 &m)
	{
		return Matrix4x4(-m.rows[0], -m.rows[1], -m.rows[2], -m.rows[3]);
	}

	#define rcDot4(r, c) (m.rows[r].x * n.rows[0][c] + m.rows[r].y * n.rows[1][c] + m.rows[r].z * n.rows[2][c] + m.rows[r].w * n.rows[3][c])

	Matrix4x4 operator * (const Matrix4x4 &m, const Matrix4x4 &n)
	{
		return Matrix4x4(
			rcDot4(0, 0), rcDot4(0, 1), rcDot4(0, 2), rcDot4(0, 3),
			rcDot4(1, 0), rcDot4(1, 1), rcDot4(1, 2), rcDot4(1, 3),
			rcDot4(2, 0), rcDot4(2, 1), rcDot4(2, 2), rcDot4(2, 3),
			rcDot4(3, 0), rcDot4(3, 1), rcDot4(3, 2), rcDot4(3, 3));
	}

	Vector4 operator * (const Matrix4x4 &m, const Vector4 &v)
	{
		return Vector4(dot(m.rows[0], v), dot(m.rows[1], v), dot(m.rows[2], v), dot(m.rows[3], v));
	}

	Matrix4x4 operator * (const Matrix4x4 &m, const float x)
	{
		return Matrix4x4(m.rows[0] * x, m.rows[1] * x, m.rows[2] * x, m.rows[3] * x);
	}
	
	Matrix4x4 transpose(const Matrix4x4 &m)
	{
		return Matrix4x4(
			m.rows[0].x, m.rows[1].x, m.rows[2].x, m.rows[3].x,
			m.rows[0].y, m.rows[1].y, m.rows[2].y, m.rows[3].y,
			m.rows[0].z, m.rows[1].z, m.rows[2].z, m.rows[3].z,
			m.rows[0].w, m.rows[1].w, m.rows[2].w, m.rows[3].w);
	}
}
